/*
 * elasticenergy.cpp
 *
 *      Author: Jerome RD Soine and Christoph A Brand
 *      Institution: Schwarz goup, Institute for Theoretical Physics, Heidelberg University, Germany
 */

#include "../../../../../../include/base/cell_models/ACM/energy/elastic/activecableenergy.h"
#include "../../../../../../include/base/cell_models/ACM/geometry/base/vertex.h"
#include "../../../../../../include/base/cell_models/ACM/geometry/base/edge.h"
#include "../../../../../../include/base/cell_models/ACM/ActiveCableModel.h"

#include <math.h>
#include <iostream>
#include <list>
#include <cstdlib>
#include <cmath>

ActiveCableEnergy::ActiveCableEnergy(GeomObject * G, double oneDYoungsModulus) : CableEnergy(G, oneDYoungsModulus, 1.0) {
	 cutoffFraction = 0.01;
}

double ActiveCableEnergy::getEnergy() {
	double energy = 0;
	vector<Edge*> edges = *g->getEdges();
	for (unsigned int i = 0; i < edges.size(); i++) {
		energy += getEnergyOfEdge(edges[i]);
	}
	return energy;
}

double ActiveCableEnergy::getEnergyOfEdge(Edge * edge){
	double length = edge->toVector().abs();
	double restlength = edge->getRestlength();
	double tension = edge->getValueOfAttribute(ActiveCableModel::FIELD_TENSION);

	if(length>restlength){
		double e1 = 0.5 * tension * restlength * cutoffFraction;
		double e2 = tension * (length-restlength*cutoffFraction);
		double e3 = 0.5 * spring_constant/restlength * (length - restlength)* (length - restlength);
		return e1 + e2 + e3;
	}	//else return  tension * (length-restlength);
	else if(length <= restlength && length > cutoffFraction * restlength) {
		double e1 = 0.5 * tension * restlength * cutoffFraction;
		double e2 = tension *(length- restlength*cutoffFraction);
		return e1+e2;
	}
	else if(length <= cutoffFraction * restlength){
		double e1 = 0.5 * tension / (cutoffFraction*restlength) * length * length;
		return e1;
	} else {
		cout << "Undefined length for active cable: " << length << endl;
		cout << "P1: (#"<<edge->getHead()->getIndex()<<") " << edge->getHead()->toString() << " - P2: (#"<<edge->getTail()->getIndex()<<") " << edge->getTail()->toString() << endl;
		cout << "Edge #" <<edge->getIndex()<< " with restlength " << edge->getRestlength() << "and tension " << tension<<endl;
		exit(1);
	}
}

double ActiveCableEnergy::getForceOfEdge(Edge * edge){
	double length = edge->toVector().abs();
	double restlength = edge->getRestlength();
	if(restlength*restlength < 0.00001) {
		cout << "RESTLENGTH ZERO: ";
		cout << "P1: (#"<<edge->getHead()->getIndex()<<") " << edge->getHead()->toString() << " - P2: (#"<<edge->getTail()->getIndex()<<") " << edge->getTail()->toString() << endl;
		cout << "Returning force zero." << endl;
		return 0;
	}
	double tension = edge->getValueOfAttribute(ActiveCableModel::FIELD_TENSION);
	if(std::isnan(tension)){
		cout << "TENSION NOT DEFINED: " << endl;
		cout << "P1: (#"<<edge->getHead()->getIndex()<<") " << edge->getHead()->toString() << " - P2: (#"<<edge->getTail()->getIndex()<<") " << edge->getTail()->toString() << endl;
		cout << "Edge #" <<edge->getIndex()<< " with restlength " << edge->getRestlength() << "and tension " << tension<<endl;
		cout << endl;
	}
	if(length>restlength){
		return spring_constant/restlength * (length - restlength) + tension;
	}
	//else return tension;
	else if(length <= restlength && length > cutoffFraction * restlength) {
		return tension;
	}
	else if(length <= cutoffFraction * restlength){
		return tension / (cutoffFraction*restlength) * length;
	} else {
		cout << "Undefined length for active cable: " << length << endl;
		cout << "P1: (#"<<edge->getHead()->getIndex()<<") " << edge->getHead()->toString() << " - P2: (#"<<edge->getTail()->getIndex()<<") " << edge->getTail()->toString() << endl;
		cout << "Edge #" <<edge->getIndex()<< " with restlength " << edge->getRestlength() << "and tension " << tension<<endl;
		exit(1);
	}
}

Vertex ActiveCableEnergy::getGradient(Vertex & v) {
	Vertex gradient(0, 0, 0);
	vector<Edge*> connectedEdges = *v.getEdgesConnectingVertex();

	for (unsigned int i = 0; i < connectedEdges.size(); i++) {
		double force = getForceOfEdge(connectedEdges[i]);
		Vertex * head = connectedEdges[i]->getHead();
		Vertex direction = connectedEdges[i]->toVector();
		if(head != &v) direction = direction * (-1); // Gradient in direction of growing energy
		direction = direction / direction.abs();
		gradient += direction * force;
	}
	return gradient;
}

